#include <stdafx.h>
#include "IPC.h"


ChainCode& ChainCode::operator=(const ChainCode& c)
{
	this->nX = c.nX;
	this->nY = c.nY;
	this->nValue = c.nValue;
	return *this;
}
bool ChainCode::operator==(const ChainCode&c)
{
	return(this->nX==c.nX && this->nY==c.nY && this->nValue==c.nValue);
}
ImageProcessing::ImageProcessing()
{
	nCountFeaturePoints=0;
	Xmax = 0;
	Ymax = 0;
	Xmin = 10000;
	Ymin = 10000;
	pHSV = NULL;
	pImage = NULL;
	pImageGray = NULL;
}
ImageProcessing::~ImageProcessing()
{
	pHSV = NULL;
	delete pHSV;
	pImage = NULL;
	delete pImage;
	
}
void ImageProcessing::Reset()
{
	nCountFeaturePoints=0;
	Xmax = 0;
	Ymax = 0;
	Xmin = 10000;
	Ymin = 10000;
	pHSV = NULL;
	pImage = NULL;
	pImageGray = NULL;
	lChain.clear();
	lTurningPoint.clear();
}
bool ImageProcessing::LoadImage(char* filename)
{
	Reset();
	pImage = (IplImage*)cvLoadImage(filename);
	if (!pImage)
	{
		nErrorID = 1;
		return false;
	}
	//cvSmooth( pImage, pImage, CV_GAUSSIAN, 13, 13 );
	//PaintBackground();
	pImageOri = (IplImage*)cvLoadImage(filename);

	h = pImage->height;
	w = pImage->width;
	byteWidth = pImage->widthStep;
	nCountFeaturePoints=0;
	return true;
}
//H: [0,360]
//S: [0,255]
//V: [0,255]
void ImageProcessing::ConvertRGBtoHSV(unsigned char r, unsigned char g,unsigned char b, HSV &out)
{
	//V=max(R,G,B)
	//S=(V-min(R,G,B))*255/V   ,if V!=0, 
	//  0                      ,otherwise
	//       (G - B)*60/S,  if V=R
	//H= 180+(B - R)*60/S,  if V=G
	//	 240+(R - G)*60/S,  if V=B
	//if H<0 then H=H+360
	unsigned char iMax;
	unsigned char iMin;
	iMax = (r>g)?r:g;
	iMin = (r>g)?g:r;
	iMax = (b>iMax)?b:iMax;
	iMin = (b<iMin)?b:iMin;
	
	//calculate V
	out.V = iMax;

	//calculate S
	if(iMax!=0)
		out.S = (iMax - iMin)*255/iMax;
	else
		out.S = 0;

	//calculate H
	if(out.S==0)
		out.H = 0;// undefine or any
	else
	{
		float fHue;
		float fDelta;
		fDelta = iMax - iMin;
		if(iMax == r)
			fHue = (g-b)/fDelta;
		else if(iMax == g)
			fHue = 2.f + (b-r)/fDelta;
		else
			fHue = 4.f + (r-g)/fDelta;
		fHue*=60;
		if(fHue<0.f)
			fHue += 360.f;
		out.H = (int)fHue;
	}

}

void ImageProcessing::ConvertHSVtoRGB(HSV in, unsigned char &r,unsigned char &g, unsigned char &b)
{
	if(in.S <= 0)
	{
		r = in.V;
		g = in.V;
		b = in.V;
		return;
	}
	else
	{
		float fHue, fSaturation, fValue;
		int i;
		float f;
		float p, q, t;

		if(in.H==360)
			in.H = 0;
		fHue = (float)in.H/60.f;
		i = (int)fHue;
		f = fHue - floor((float)i);

		fValue = (float)in.V/255.f;
		fSaturation = (float)in.S/255.f;

		p = fValue*(1.f	- fSaturation);
		q = fValue*(1.f - (fSaturation*f));
		t = fValue*(1.f - (fSaturation*(1.f - f)));

		switch(i)
		{
			case 0:
				r = (unsigned char)floor(fValue*255+0.5);
				g = (unsigned char)floor(t*255+0.5);
				b = (unsigned char)floor(p*255+0.5);
				break;
			case 1:
				r = (unsigned)floor(q*255+0.5);
				g = (unsigned char)floor(fValue*255+0.5);
				b = (unsigned char)floor(p*255+0.5);
				break;
			case 2:
				r = (unsigned char)floor(p*255+0.5);
				g = (unsigned char)floor(fValue*255+0.5);
				b = (unsigned char)floor(t*255+0.5);
				break;
			case 3:
				r = (unsigned char)floor(p*255+0.5);
				g = (unsigned char)floor(q*255+0.5);
				b = (unsigned char)floor(fValue*255+0.5);
				break;
			case 4:
				r = (unsigned char)floor(t*255+0.5);
				g = (unsigned char)floor(p*255+0.5);
				b = (unsigned char)floor(fValue*255+0.5);
				break;
			case 5:
				r = (unsigned char)floor(fValue*255.f+0.5f);
				g = (unsigned char)floor(p*255.f+0.5f);
				b = (unsigned char)floor(q*255.f+0.5f);
				break;
		}
	}
	//r = ((unsigned char)r>255)?255:r;
	//g = ((unsigned char)g>255)?255:g;
	//b = ((unsigned char)b>255)?255:b;
	//r = ((unsigned char)r<0)?0:r;
	//g = ((unsigned char)g<0)?0:g;
	//b = ((unsigned char)b<0)?0:b;
	return;
}
bool ImageProcessing::RGBtoHSV()
{
	if(!pImage)
		return false;

	pHSV = new HSV[h*w];

	for (int y=0; y<h; y++)
	{
		long int R = y*byteWidth;
		for(int x=0 ; x<w; x++)
		{
			long int idxPixel = R+ x*3;
			unsigned char b = pImage->imageData[idxPixel + 0];
			unsigned char g = pImage->imageData[idxPixel + 1];
			unsigned char r = pImage->imageData[idxPixel + 2];

			long int idx = y*w+x;
			ConvertRGBtoHSV(r,g,b,pHSV[idx]);
			//ConvertHSVtoRGB(pHSV[idx],r,g,b);//test
		}
	}
		
	return true;
};
bool ImageProcessing::HSVtoRGB()
{
	if(!pHSV)
		return false;
	
	for (int y=0; y<h; y++)
	{
		long int R = y*byteWidth;
		for(int x=0 ; x<w; x++)
		{
			long int idxPixel = R+ x*3;
			long int idx = y*w+x;
		
			unsigned char r,g,b;
			ConvertHSVtoRGB(pHSV[idx],r,g,b);

			pImage->imageData[idxPixel + 0] = (char)b;
			pImage->imageData[idxPixel + 1] = (char)g;
			pImage->imageData[idxPixel + 2] = (char)r;
		}
	}
		
	return true;
}
int ImageProcessing::FindChainCode(int x, int y,int &nextX, int &nextY, unsigned char preValue,unsigned char grayValue)
{
	int i,j;
	long int idx;
	int tx, ty;
	unsigned char revIdx;

	i=1;
	j=0;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (0+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
			return 0;
	}

	i=1;
	j=-1;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (7+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
			return 7;
	}

	i=0;
	j=-1;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (6+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
		return 6;
	}

	i=-1;
	j=-1;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (5+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
		return 5;
	}

	i=-1;
	j=0;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (4+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
		return 4;
	}

	i=-1;
	j=1;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (3+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
		return 3;
	}

	i=0;
	j=1;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (2+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
		return 2;
	}

	i=1;
	j=1;
	tx = x+i;
	ty = y-j;
	idx = ty*pImageGray->widthStep + tx;
	if ((unsigned char)pImageGray->imageData[idx]==grayValue)
	{
		nextX = tx;
		nextY = ty;
		revIdx = (1+4)%8;

		if( preValue!=revIdx
			&&preValue!=(revIdx+1)%8
			&&preValue!=(revIdx-1)%8
			)
		return 1;
	}
	return 255;
}
bool ImageProcessing::FilterOnHSV(int Hmin, int Hmax, int Smin, int Smax, int Vmin, int Vmax)
{
	if(!pHSV)
		return false;

	for(int x = 0;x<w;x++)
		for(int y=0;y<h;y++)
		{
			long int idx = y*w+x;
			/*
			if(pHSV[idx].V<90 && (unsigned char)(pImage->imageData[y*pImage->widthStep + x*3+ 0])<50)
			{
				pHSV[idx].V = 255;
				pHSV[idx].S = 0;
				pHSV[idx].H = 0;
				continue;
			}//*/
			if(		pHSV[idx].H>=Hmin && pHSV[idx].H<=Hmax
				&&	pHSV[idx].S>=Smin && pHSV[idx].S<=Smax
			)
				pHSV[idx].V = 0;
			else 
				pHSV[idx].V = 255;

			pHSV[idx].H = 0;
			pHSV[idx].S = 0;
		}
	return true;
}
void ImageProcessing::SetupGrayImage(unsigned char opt)
{
	int width;
	int height;
	int depth;
	int channels;

	if(!pImage)
		return;
	if(opt==0)
	{
		width = pImage->width;
		height = pImage->height;
		depth = pImage->depth;
		channels = pImage->nChannels;
	}
	pImageGray = cvCreateImage(cvSize(width,height),IPL_DEPTH_8U,1);
	cvCvtColor(pImage,pImageGray,CV_BGR2GRAY);
}
bool ImageProcessing::MakeCannyEdge()
{
	if(!pImage)
		return false;
	//cvPyrDown(pImageGray,pImageGray);
	
	cvCanny(pImageGray,pImageGray,70,100,3);
}
//Chaincode
bool ImageProcessing::CreateChainCode()
{
	if(!pImageGray)
	{
		nErrorID = 51;
		return false;
	}
	
	ChainCode chain;
	bool bFoundRoot = false;
	bool bFinish =false;
	unsigned char preValue = 0;
	int nRootX, nRootY; //root position.
	int nNextX, nNextY; //next pixel on contour
	nNextX = 0;
	nNextY = 0;
	//find root
	for (int y=0; y<h; y++)
	{
		if(bFoundRoot)
			break;
		long int R = y*pImageGray->widthStep;
		for(int x=0 ; x<w; x++)
		{
			long int idxPixel = R+ x*1;
			unsigned char gray = pImageGray->imageData[idxPixel];
			
			if (gray==255)
			{
				bFoundRoot = true;
				chain.nX = nRootX = x;
				chain.nY = nRootY = y;
				chain.nValue = FindChainCode(x,y,nNextX,nNextY,preValue,255);
				preValue = chain.nValue;
				lChain.push_back(chain);
				//update Xmax, Xmin, Ymax, Ymin
				Ymin = y;
				Ymax = (Ymax<y)?y:Ymax;
				Xmax = (Xmax<x)?x:Xmax;
				Xmin = (Xmin>x)?x:Xmin;
				break;
			}
		}
	}
	if(!bFoundRoot)
	{
		nErrorID = 52;
		return false;
	}
	//trace contour
	while(nRootX!=nNextX || nRootY!=nNextY)
	{
		int x = nNextX;
		int y = nNextY;
		chain.nX = x;
		chain.nY = y;
		chain.nValue = FindChainCode(x,y,nNextX,nNextY,preValue,255);
		if(chain.nValue==255)
		{

			pImageGray->imageData[y*pImageGray->widthStep+x]=150;
			nErrorID = 53;
			cout<<"/n[ERROR]: Can not fully make chain code  because the boudary is corrupted.\n";
			break;
		}
		preValue = chain.nValue;
		lChain.push_back(chain);
		 
		//for checking
		pImageGray->imageData[y*pImageGray->widthStep+x]=50;
		//update Xmax, Xmin, Ymax, Ymin
		Ymax = (Ymax<y)?y:Ymax;
		Xmax = (Xmax<x)?x:Xmax;
		Xmin = (Xmin>x)?x:Xmin;
	}
	
	//clear the image
	for (int y=0; y<h; y++)
		for (int x=0; x<w; x++)
		{
			if(pImageGray->imageData[y*pImageGray->widthStep+x]==(char)255 
				&&(pImageGray->imageData[y*pImageGray->widthStep+x+1]!=(char)0 || pImageGray->imageData[y*pImageGray->widthStep+x-1]!=(char)0)
				)
				pImageGray->imageData[y*pImageGray->widthStep+x]=0;
		}
		//show bouding box
		/*
		for (int y=0; y<h; y++)
			for (int x=0; x<w; x++)
			{
				if(x==Xmin || x==Xmax || y== Ymin || y==Ymax)
					pImageGray->imageData[y*pImageGray->widthStep+x]=255;
			}
			//*/
	//refine chain code and image
	list<ChainCode>::iterator iChain;
	
	for( iChain=lChain.begin();iChain!=--(--lChain.end());++iChain)
	{
		ChainCode temp0,temp1,temp2;
		temp0 = *(iChain);
		temp1 = *(++iChain);
		temp2 = *(++iChain);

		if((int)abs(temp0.nY-temp1.nY)==1 && (int)abs(temp1.nY-temp2.nY)==1 && temp0.nX==temp2.nX && temp0.nX!=temp1.nX)
		{
				--iChain;
				--iChain;
				pImageGray->imageData[temp1.nY*pImageGray->widthStep+temp1.nX]=0;
				temp1.nX = temp0.nX;
				pImageGray->imageData[temp1.nY*pImageGray->widthStep+temp1.nX]=50;
				temp0.nValue = FindChainCode(temp0.nX,temp0.nY,temp1.nX,temp1.nY,(temp0.nY>temp1.nY)?2:6,50);
				lChain.insert(iChain,temp0);
				iChain=lChain.erase(iChain);
				temp1.nValue = FindChainCode(temp1.nX,temp1.nY,temp2.nX,temp2.nY,(temp1.nY>temp2.nY)?2:6,50);
				lChain.insert(iChain,temp1);
				iChain=lChain.erase(iChain);
			
		};
		--(--iChain);
	}
	for( iChain=lChain.begin();iChain!=--(--(--lChain.end()));++iChain)
	{
		ChainCode temp0,temp1,temp2,temp3;
		temp0 = *(iChain);
		temp1 = *(++iChain);
		temp2 = *(++iChain);
		temp3 = *(++iChain);
		if((int)abs(temp0.nY-temp1.nY)==1 && (int)abs(temp1.nY-temp2.nY)==1 && (int)abs(temp2.nY-temp3.nY)==1 
			&& temp0.nX==temp3.nX && temp0.nX!=temp1.nX && temp2.nX==temp1.nX)
		{
				--iChain;
				--iChain;
				--iChain;
				pImageGray->imageData[temp1.nY*pImageGray->widthStep+temp1.nX]=0;
				temp1.nX = temp0.nX;
				pImageGray->imageData[temp1.nY*pImageGray->widthStep+temp1.nX]=50;
				temp0.nValue = FindChainCode(temp0.nX,temp0.nY,temp1.nX,temp1.nY,(temp0.nY>temp1.nY)?2:6,50);
				lChain.insert(iChain,temp0);
				iChain=lChain.erase(iChain);

				pImageGray->imageData[temp2.nY*pImageGray->widthStep+temp2.nX]=0;
				temp2.nX = temp0.nX;
				pImageGray->imageData[temp2.nY*pImageGray->widthStep+temp2.nX]=50;
				temp1.nValue = FindChainCode(temp1.nX,temp1.nY,temp2.nX,temp2.nY,(temp1.nY>temp2.nY)?2:6,50);
				lChain.insert(iChain,temp1);
				iChain=lChain.erase(iChain);

				temp2.nValue = FindChainCode(temp2.nX,temp2.nY,temp3.nX,temp3.nY,(temp2.nY>temp3.nY)?2:6,50);
				lChain.insert(iChain,temp2);
				iChain=lChain.erase(iChain);
			
		};
		--(--(--iChain));
	}
	for( iChain=lChain.begin();iChain!=--(--(--(--lChain.end())));++iChain)
	{
		ChainCode temp0,temp1,temp2,temp3,temp4;
		temp0 = *(iChain);
		temp1 = *(++iChain);
		temp2 = *(++iChain);
		temp3 = *(++iChain);
		temp4 = *(++iChain);
		if((int)abs(temp0.nY-temp1.nY)==1 && (int)abs(temp1.nY-temp2.nY)==1 && (int)abs(temp2.nY-temp3.nY)==1 && (int)abs(temp3.nY-temp4.nY)==1 
			&& temp0.nX==temp4.nX && temp0.nX!=temp1.nX && temp1.nX==temp2.nX && temp2.nX == temp3.nX)
		{
				--iChain;
				--iChain;
				--iChain;
				--iChain;
				pImageGray->imageData[temp1.nY*pImageGray->widthStep+temp1.nX]=0;
				temp1.nX = temp0.nX;
				pImageGray->imageData[temp1.nY*pImageGray->widthStep+temp1.nX]=50;
				temp0.nValue = FindChainCode(temp0.nX,temp0.nY,temp1.nX,temp1.nY,(temp0.nY>temp1.nY)?2:6,50);
				lChain.insert(iChain,temp0);
				iChain=lChain.erase(iChain);

				pImageGray->imageData[temp2.nY*pImageGray->widthStep+temp2.nX]=0;
				temp2.nX = temp0.nX;
				pImageGray->imageData[temp2.nY*pImageGray->widthStep+temp2.nX]=50;
				temp1.nValue = FindChainCode(temp1.nX,temp1.nY,temp2.nX,temp2.nY,(temp1.nY>temp2.nY)?2:6,50);
				lChain.insert(iChain,temp1);
				iChain=lChain.erase(iChain);

				pImageGray->imageData[temp3.nY*pImageGray->widthStep+temp3.nX]=0;
				temp3.nX = temp0.nX;
				pImageGray->imageData[temp3.nY*pImageGray->widthStep+temp3.nX]=50;
				temp2.nValue = FindChainCode(temp2.nX,temp2.nY,temp3.nX,temp3.nY,(temp2.nY>temp3.nY)?2:6,50);
				lChain.insert(iChain,temp2);
				iChain=lChain.erase(iChain);


				temp3.nValue = FindChainCode(temp3.nX,temp3.nY,temp4.nX,temp4.nY,(temp3.nY>temp4.nY)?2:6,50);
				lChain.insert(iChain,temp3);
				iChain=lChain.erase(iChain);
			
		};
		--(--(--(--iChain)));
	}
	
	//*/
	/*
	CvMemStorage *storage;
	storage = cvCreateMemStorage(0);

	int numContours = cvFindContours(pImageGray,storage,(CvSeq**)&pChain,sizeof(*pChain),CV_RETR_LIST,CV_CHAIN_CODE);
	Test_PrintChainCode();
	//*/
	return true;
}
void ImageProcessing::DrawMarkerPoint(IplImage *img,int centerX, int centerY,int cellWidth, int cellHeight)
{
	int nW = cellWidth/2;
	int nH = cellHeight/2;
	int x = centerX;
	int y = centerY;
	for(int i = -nW;i<=nW;i++)
		for(int j = -nH;j<=nH;j++)
		{
			x +=i;
			y +=j;
			img->imageData[y*img->widthStep + x*3 + 0]  = 0;
			img->imageData[y*img->widthStep + x*3 + 1]  = 0;
			img->imageData[y*img->widthStep + x*3 + 2]  = 200;
			x-=i;
			y-=j;
		}
}
void ImageProcessing::ExtractFeaturesforSide()
{
	//Extract feature points
	ChainCode ccTemp;//temporary variable
	bool bFound = false;
	nCountFeaturePoints=1;
	int referedIdx;
	int bodyLength = Ymax - Ymin +1;
	int H = bodyLength/8;
	//Tran
	for( iChain=++lChain.begin();iChain!=lChain.end();++iChain)
	{
		if(iChain->nValue==7)
		{
			
			iChain++;
			if(iChain->nValue==6)
			{
				ChainCode temp;
				temp = *(iChain);
				iChain++;
				if(iChain->nValue==6)
				{
					features[nCountFeaturePoints].type = SideTran1;
					features[nCountFeaturePoints].x = temp.nX;
					features[nCountFeaturePoints].y = temp.nY;
					features[nCountFeaturePoints].nCCValue= temp.nValue;
					pImageGray->imageData[temp.nY*pImageGray->widthStep+temp.nX]=255;
					referedIdx = nCountFeaturePoints;
					nCountFeaturePoints++;

					for(int i=temp.nX-2;i>Xmin;i--)
					{
						if((unsigned char)(pImageGray->imageData[temp.nY*pImageGray->widthStep+i])>0)
						{
							features[nCountFeaturePoints].type = SideTran2;
							features[nCountFeaturePoints].x = i;
							features[nCountFeaturePoints].y = temp.nY;
							//features[nCountFeaturePoints].nCCValue= temp.nValue;
							pImageGray->imageData[temp.nY*pImageGray->widthStep+i]=255;
							referedIdx = nCountFeaturePoints;
							nCountFeaturePoints++;
							break;
						}
					}
					break;
				}
				iChain--;
			}
			iChain--;
		}
	}
	//Hom~ co
	for( iChain=++lChain.begin();iChain!=lChain.end();++iChain)
	{
		if(iChain->nY>(features[referedIdx].y + H))
		{
			if(iChain->nValue==7)
			{
				ccTemp = *(iChain);
				features[nCountFeaturePoints].type = SideHomCo;
				features[nCountFeaturePoints].x = ccTemp.nX;
				features[nCountFeaturePoints].y = ccTemp.nY;
				features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
				pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
				referedIdx = nCountFeaturePoints;
				nCountFeaturePoints++;
				break;
			}
		}
		
	}

	//Dinh Nguc
	bFound = false;
	for( iChain=++lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY>features[referedIdx].y && (iChain->nY<(features[referedIdx].y+H)) )
		{
			list<ChainCode>::iterator it;
			float d = 0;
			Feature temp;
			int max = 0;
			for(it=iChain;it!=lTurningPoint.end();it++)
			{
				d = it->nX - Xmin + 1;
				if(max<d)
				{
					max = d;
					temp.x = it->nX;
					temp.y = it->nY;
					temp.nCCValue = it->nValue;
				}
				if(d<max)
				{
					features[nCountFeaturePoints].type = SideDinhNguc1;
					features[nCountFeaturePoints].x = temp.x;
					features[nCountFeaturePoints].y = temp.y;
					features[nCountFeaturePoints].nCCValue = temp.nCCValue;
					pImageGray->imageData[temp.y*pImageGray->widthStep+temp.x]=255;
					nCountFeaturePoints++;
					break;
				}
			}
			
			for(int i = temp.x;i>0;--i)
			{
				if((unsigned char)(pImageGray->imageData[temp.y*pImageGray->widthStep+i])>0)
				{
					features[nCountFeaturePoints].type = SideDinhNguc2;
					features[nCountFeaturePoints].x = i;
					features[nCountFeaturePoints].y = temp.y;
					//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
					pImageGray->imageData[temp.y*pImageGray->widthStep+i]=255;
					referedIdx = nCountFeaturePoints;
					nCountFeaturePoints++;
				}
			}
			break;
		}

	}
	//Mong^
	//thuat toan to dan

	bFound = false;
	for( iChain=++lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY>(features[referedIdx].y+H/2))
		{
			list<ChainCode>::iterator it;
			float d = 0;
			Feature temp;
			int min = 10000;
			int x2;
			for(it=iChain;it->nY<=(features[referedIdx].y+2*H);it++)
			{
				int x2temp=0;
				for(int x = it->nX-2; x>=Xmin;x--)
					if((unsigned char)(pImageGray->imageData[it->nY*pImageGray->widthStep+x])>0)
					{
						x2temp = x;
						break;
					}
				d = x2temp - Xmin + 1;
				if(min>d)
				{
					min = d;
					x2 = x2temp;
					temp.x = it->nX;
					temp.y = it->nY;
					temp.nCCValue = it->nValue;	
				}
			}
			features[nCountFeaturePoints].type = SideMong1;
			features[nCountFeaturePoints].x = temp.x;
			features[nCountFeaturePoints].y = temp.y;
			features[nCountFeaturePoints].nCCValue = temp.nCCValue;
			pImageGray->imageData[temp.y*pImageGray->widthStep+temp.x]=255;
			referedIdx = nCountFeaturePoints;
			nCountFeaturePoints++;

			features[nCountFeaturePoints].type = SideMong2;
			features[nCountFeaturePoints].x = x2;
			features[nCountFeaturePoints].y = temp.y;
			features[nCountFeaturePoints].nCCValue = 8;
			pImageGray->imageData[temp.y*pImageGray->widthStep+x2]=255;
			nCountFeaturePoints++;
			break;
		}
	}
	//dau^` goi^'
	bFound = false;
	for( iChain=++lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY>(features[referedIdx].y+3*H/2))
		{
			if(iChain->nValue==6)
			{
				iChain++;
				if(iChain->nValue==6)
				{
					iChain++;
					if(iChain->nValue==6)
					{
						iChain++;
						if(iChain->nValue==6)
						{
							iChain++;
							if(iChain->nValue==6)
							{
								iChain++;
								if(iChain->nValue==5)
								{
									ChainCode temp;
									temp = *(iChain);
							
									features[nCountFeaturePoints].type = SideDauGoi1;
									features[nCountFeaturePoints].x = temp.nX;
									features[nCountFeaturePoints].y = temp.nY;
									features[nCountFeaturePoints].nCCValue= temp.nValue;
									pImageGray->imageData[temp.nY*pImageGray->widthStep+temp.nX]=255;
									referedIdx = nCountFeaturePoints;
									nCountFeaturePoints++;
									for(int i = temp.nX;i>0;--i)
									{
										if((unsigned char)(pImageGray->imageData[temp.nY*pImageGray->widthStep+i])>0)
										{
											features[nCountFeaturePoints].type = SideDauGoi2;
											features[nCountFeaturePoints].x = i;
											features[nCountFeaturePoints].y = temp.nY;
											//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
											pImageGray->imageData[temp.nY*pImageGray->widthStep+i]=255;
											nCountFeaturePoints++;
											break;
										}
									}
									break;
								}
								iChain--;
							}
							iChain--;
						}
						iChain--;
					}
					iChain--;
				}

				iChain--;
			}			
		}
	}
	//bap' chan^
	//phuong phap to dan bang
	bFound = false;
	for( iChain=++lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY>(features[referedIdx].y+2*H/3))
		{
			list<ChainCode>::iterator it;
			float d = 0;
			Feature temp;
			int x2;
			for(it=iChain;it!=lTurningPoint.end();it++)
			{
				int x = it->nX;
				int y = it->nY;

				for(int i = x-2; i>0; --i)
				{
					if((unsigned char)(pImageGray->imageData[y*pImageGray->widthStep+i])>0)
					{
						//if(pImageGray->imageData[y*pImageGray->widthStep+(i-1)]!=0)
						//	--i;

						if(d<=(x-i+1))
						{
							d = x-i+1;
							temp.x = x;
							temp.y = y;
							x2 = i;
							temp.nCCValue = it->nValue;
						}
						if(d>(x-i+1))
						{
							bFound = true;
							//Diem mong^ ben phai
							features[nCountFeaturePoints].type = SideBapChan1;
							features[nCountFeaturePoints].x = temp.x;
							features[nCountFeaturePoints].y = temp.y;
							features[nCountFeaturePoints].nCCValue = temp.nCCValue;
							pImageGray->imageData[temp.y*pImageGray->widthStep+temp.x]=255;
							referedIdx = nCountFeaturePoints;
							nCountFeaturePoints++;
							//Dinh nguc 2 ben trai
							features[nCountFeaturePoints].type = SideBapChan2;
							features[nCountFeaturePoints].x = x2;
							features[nCountFeaturePoints].y = temp.y;
							//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
							pImageGray->imageData[temp.y*pImageGray->widthStep+x2]=255;
							nCountFeaturePoints++;
						}
						break;
					}
				}
				if(bFound)
					break;
			}
			if(bFound)
				break;
		}
	}

	//ban` chan
	int maxX = 0;
	int minX = 10000;
	Feature tempMax;
	Feature tempMin;
	for(int y = Ymax;y>(Ymax - bodyLength/8);--y)
	{
		for(int x = Xmin; x<=Xmax;x++)
		{
			if((unsigned char)(pImageGray->imageData[y*pImageGray->widthStep+x])>0)
			{
				if(maxX<x)
				{
					maxX = x;
					tempMax.x = maxX;
					tempMax.y = y;
				}
				if(minX>x)
				{
					minX = x;
					tempMin.x = minX;
					tempMin.y = y;
				}
			}
		}
	}
	features[nCountFeaturePoints].type = SideBanChan1;
	features[nCountFeaturePoints].x = tempMax.x;
	features[nCountFeaturePoints].y = tempMax.y;
	//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
	pImageGray->imageData[tempMax.y*pImageGray->widthStep+tempMax.x]=255;
	referedIdx = nCountFeaturePoints;
	nCountFeaturePoints++;
	features[nCountFeaturePoints].type = SideBanChan2;
	features[nCountFeaturePoints].x = tempMin.x;
	features[nCountFeaturePoints].y = tempMin.y;
	//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
	pImageGray->imageData[tempMin.y*pImageGray->widthStep+tempMin.x]=255;
	referedIdx = nCountFeaturePoints;
	nCountFeaturePoints++;

	//xu*o*ng hong
	for(int i=1;i<nCountFeaturePoints;i++)
	{
		if(features[i].type==SideMong2)
		{
			referedIdx=i;
			break;
		}
	}
	bFound = false;
	int d = 1000;
	for(iChain=lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if( iChain->nY==features[referedIdx].y && iChain->nX==features[referedIdx].x)
		{
			list<ChainCode>::iterator it;
			int dmin = 10000;
			int x1 = 0;
			int y1 = 0;
			int x2 = 0;
			int y2 = 0;
			for(it=iChain++;it!=lTurningPoint.end();it++)
			{
				if(it->nY<(features[referedIdx].y-H/2))
					break;
				int i;
				for(i = it->nX+2; i<=Xmax; i++)
				{
					if((unsigned char)(pImageGray->imageData[it->nY*pImageGray->widthStep+i])>0 )
					{
						d = i-it->nX+1;
						break;
					}
				}
				if(d<=dmin)
				{
					dmin=d;
					x1 = it->nX;
					y1 = it->nY;
					x2 = i;
					y2 = y1;
				}
			}
			features[nCountFeaturePoints].type = SideXuongHong2;
			features[nCountFeaturePoints].x = x1;
			features[nCountFeaturePoints].y = y1;
			//features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
			pImageGray->imageData[y1*pImageGray->widthStep+x1]=255;
			referedIdx = nCountFeaturePoints;
			nCountFeaturePoints++;
			
			features[nCountFeaturePoints].type = SideXuongHong1;
			features[nCountFeaturePoints].x = x2;
			features[nCountFeaturePoints].y = y2;
			//features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
			pImageGray->imageData[y2*pImageGray->widthStep+x2]=255;
			referedIdx = nCountFeaturePoints;
			nCountFeaturePoints++;
	
			break;
		}
	}
	//co^ chan^

	bFound = false;
	for(iChain=lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY>(Ymax-H))
		{
			int d = 10000;
			list<ChainCode>::iterator it;
			
			for(it=iChain++;it!=lTurningPoint.end();it++)
			{
				int x = it->nX;
				int y = it->nY;
				for(int i = x-2; i>Xmin; i--)
				{
					if((unsigned char)(pImageGray->imageData[y*pImageGray->widthStep+i])>0 )
					{
						if(d>=(x-i+1))
						{
							d = x-i+1;
							ccTemp.nX = x;
							ccTemp.nY = y;
							ccTemp.nValue = it->nValue;

						}
						if(d<(x-i+1))
						{
							bFound = true;

							features[nCountFeaturePoints].type = SideCoChan1;
							features[nCountFeaturePoints].x = ccTemp.nX;
							features[nCountFeaturePoints].y = ccTemp.nY;
							features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
							pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
							referedIdx = nCountFeaturePoints;
							nCountFeaturePoints++;
							for(int i = ccTemp.nX-2;i>Xmin;--i)
							{
								if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i])>0)
								{
									if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i+1])>0)
										i++;
									features[nCountFeaturePoints].type = SideCoChan2;
									features[nCountFeaturePoints].x = i;
									features[nCountFeaturePoints].y = ccTemp.nY;
									//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
									pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]=255;
									nCountFeaturePoints++;
									break;
								}
							}

						}	
						break;
					}
				}
				if(bFound)
					break;
			}
			if(bFound)
				break;
		}
	}
	//dot song co
	int x1,y1;
	int x2,y2;
	for(int i=1;i<nCountFeaturePoints;i++)
	{
		if (features[i].type == SideHomCo)
		{
			x1 = features[i].x;
			y1= features[i].y;
			for(int j=x1;j>Xmin;j--)
			{
				if((unsigned char)(pImageGray->imageData[y1*pImageGray->widthStep+j])>0)
				{
					x2 = j;
					y2 = y1;
					break;
				}
			}
			break;
		}
		
	}
	
	for(iChain=lChain.begin();iChain!=lChain.end();iChain++)
	{
		if(iChain->nX==x2 && iChain->nY==y2)
		{
			float min=10000.f;
			for(int n=0;n<100;n++)
			{
				float d = sqrt((float)(iChain->nX-x1)*(iChain->nX-x1) + (iChain->nY - y1)*(iChain->nY-y1));
				if(d<min)
				{
					min=d;
					x2 = iChain->nX;
					y2 = iChain->nY;
				}
				iChain++;
			}
			features[nCountFeaturePoints].type = SideDotSongCo;
			features[nCountFeaturePoints].x = x2;
			features[nCountFeaturePoints].y = y2;
			pImageGray->imageData[y2*pImageGray->widthStep+x2]=255;
			nCountFeaturePoints++;
			break;
		}

	}
	//got' chan
	for(int i=1;i<nCountFeaturePoints;i++)
	{
		if(features[i].type==SideBanChan1)
		{
			referedIdx = i;
			break;
		}
	}
	bFound = false;
	for(iChain=lChain.begin();iChain!=lChain.end();iChain++)
	{
		if(iChain->nX==features[referedIdx].x && iChain->nY==features[referedIdx].y)
		{
			list<ChainCode>::iterator it;
			for(it = iChain++;it!=lChain.end();it++)
			{
				if(it->nValue==3)
				{
					ccTemp = *(it);
					it++;
					if(it->nValue==3)
					{
						bFound=true;
						features[nCountFeaturePoints].type = SideGotChan;
						features[nCountFeaturePoints].x = ccTemp.nX;
						features[nCountFeaturePoints].y = ccTemp.nY;
						pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
						nCountFeaturePoints++;
						break;
					}
					it--;
				}
			}
			if(bFound)
				break;
		}
	}
}
void ImageProcessing::ExtractFeaturesforFront()
{
	//Extract feature points
	ChainCode ccTemp;//temporary variable
	bool bFound = false;
	int referedIdx=0;
	int bodyLength = Ymax - Ymin +1;
	int H = bodyLength/8;
	nCountFeaturePoints = 1;
	for( iChain=++lChain.begin();iChain!=lChain.end();++iChain)
	{
		
		if((iChain->nY-Ymin)>2*H/3 && iChain->nValue==7)
		{
			ccTemp = (*iChain);
			iChain++;
			if(iChain->nValue==0)
			{
				features[nCountFeaturePoints].type = FrontCoVai1;
				features[nCountFeaturePoints].x = ccTemp.nX;
				features[nCountFeaturePoints].y = ccTemp.nY;
				features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
				pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
				referedIdx = nCountFeaturePoints;
				nCountFeaturePoints++;
				for(int i = ccTemp.nX-2;i>0;--i)
				{
					if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i])>0)
					{
						if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i-1])>0)
							--i;
						features[nCountFeaturePoints].type = FrontCoVai2;
						features[nCountFeaturePoints].x = i;
						features[nCountFeaturePoints].y = ccTemp.nY;
						//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
						pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]=255;
						nCountFeaturePoints++;
						break;
					}
				}
				break;
			}
			iChain--;		
		}
	}
	//Dau^` Vai
	for( iChain=++lChain.begin();iChain!=lChain.end();++iChain)
	{
		if(iChain->nY>features[referedIdx].y)
		{
			if(iChain->nValue==0)
			{
				iChain++;
				if(iChain->nValue==7)
				{
					ccTemp = *(iChain);
					iChain++;
					if(iChain->nValue==7)
					{
						
						features[nCountFeaturePoints].type = FrontDinhVai1;
						features[nCountFeaturePoints].x = ccTemp.nX;
						features[nCountFeaturePoints].y = ccTemp.nY;
						features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
						pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
						referedIdx = nCountFeaturePoints;
						nCountFeaturePoints++;
						
						for(int i = ccTemp.nX-2;i>0;--i)
						{
							if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i])>0)
							{
								if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i-1])>0)
									i--;
								features[nCountFeaturePoints].type = FrontDinhVai2;
								features[nCountFeaturePoints].x = i;
								features[nCountFeaturePoints].y = ccTemp.nY;
								//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
								pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]=255;
								nCountFeaturePoints++;
								break;
							}
						}
						break;
					}
					iChain--;
				}
				iChain--;
			}
		}
	}
	//Co^ tay ngoai 
	//chain code (6,7,7,7) hoac (7,0,0,0)
	for( iChain=++lChain.begin();iChain!=lChain.end();++iChain)
	{
		if(iChain->nY>(features[referedIdx].y+H*3/2))
		{
			unsigned char code = (iChain->nValue +1)%8;
			iChain++;
			if(iChain->nValue==code)
			{
				ccTemp = *(iChain);//detect turn.
				iChain++;
				if(iChain->nValue==code)
				{
					iChain++;
					if(iChain->nValue==code)
					{
						iChain++;
						if(iChain->nValue>=code || iChain->nValue==0)
						{
							ChainCode ccTemp2;
							if(iChain->nValue==0)
								ccTemp2=*(iChain);//detect the lowest turn.
							int i;
							int count=0;
							for( i=0;i<10;i++)
							{
								if(iChain->nValue==0)
								{
									ccTemp2 = *(iChain);
									count++;
								}
								iChain++;
								if(iChain->nValue==0)
									count++;
								else
									count=0;
								if(count>1)
								{
									ccTemp=ccTemp2;
									bFound = true;
									break;
								}
								
								if(iChain->nValue>=code|| iChain->nValue==0)
								{
									if (i==9)
										bFound = true;
								}
								else
									break;
							}
							for(int j=0;j<=i;j++)
								iChain--;
							if(bFound)
							{
								features[nCountFeaturePoints].type = FrontCoTay1;
								features[nCountFeaturePoints].x = ccTemp.nX;
								features[nCountFeaturePoints].y = ccTemp.nY;
								features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
								pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
								referedIdx = nCountFeaturePoints;
								nCountFeaturePoints++;

								list<ChainCode>::iterator it1;
								list<ChainCode>::iterator it2;
								for(it1 = iChain;it1!=lChain.end();++it1)
									if (it1->nX==ccTemp.nX && (it1->nY-10)>ccTemp.nY)
										break;

								for(it2=++it1;it2!=lChain.end();++it2)
									if(it2->nY==ccTemp.nY)
										break;
								float fMin = 10000.f;
								list<ChainCode>::iterator it;
								for(it=it1;it!=it2;++it)
								{
									float d =0.f;
									d = sqrt((float)(it->nX - iChain->nX)*(it->nX - iChain->nX) + (float)(it->nY- iChain->nY)*(it->nY- iChain->nY));
									if (d<fMin)
									{
										fMin =d;
										ccTemp.nX = it->nX;
										ccTemp.nY = it->nY;
										ccTemp.nValue = it->nValue;
									}
								}
								features[nCountFeaturePoints].type = FrontCoTay2;
								features[nCountFeaturePoints].x = ccTemp.nX;
								features[nCountFeaturePoints].y = ccTemp.nY;
								features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
								pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
								referedIdx = nCountFeaturePoints;
								nCountFeaturePoints++;
								break;
							}
							/*
							if(iChain->nValue>=code|| iChain->nValue==0)
							{
								if(iChain->nValue==0)
									ccTemp = ccTemp2;
								iChain++;
								if(iChain->nValue>=code|| iChain->nValue==0)
								{
									iChain++;
									if(iChain->nValue>=code|| iChain->nValue==0)
									{
										iChain++;
										if(iChain->nValue>=code|| iChain->nValue==0)
										{
											iChain++;
											if(iChain->nValue>=code|| iChain->nValue==0)
											{
												features[nCountFeaturePoints].type = FrontCoTay1;
												features[nCountFeaturePoints].x = ccTemp.nX;
												features[nCountFeaturePoints].y = ccTemp.nY;
												features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
												pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
												referedIdx = nCountFeaturePoints;
												nCountFeaturePoints++;

												list<ChainCode>::iterator it1;
												list<ChainCode>::iterator it2;
												for(it1 = iChain;it1!=lChain.end();++it1)
													if (it1->nX==ccTemp.nX && (it1->nY-10)>ccTemp.nY)
														break;

												for(it2=++it1;it2!=lChain.end();++it2)
													if(it2->nY==ccTemp.nY)
														break;
												float fMin = 10000.f;
												list<ChainCode>::iterator it;
												for(it=it1;it!=it2;++it)
												{
													float d =0.f;
													d = sqrt((float)(it->nX - iChain->nX)*(it->nX - iChain->nX) + (float)(it->nY- iChain->nY)*(it->nY- iChain->nY));
													if (d<fMin)
													{
														fMin =d;
														ccTemp.nX = it->nX;
														ccTemp.nY = it->nY;
														ccTemp.nValue = it->nValue;
													}
												}
												features[nCountFeaturePoints].type = FrontCoTay2;
												features[nCountFeaturePoints].x = ccTemp.nX;
												features[nCountFeaturePoints].y = ccTemp.nY;
												features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
												pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
												referedIdx = nCountFeaturePoints;
												nCountFeaturePoints++;
												break;
											}
											iChain--;
										}
										iChain--;
									}
									iChain--;
								}
								iChain--;
							}
							iChain--;
							//*/
						}
						iChain--;
						
					}
					iChain--;
				}
				iChain--;
			}
			iChain--;
		}
	}
	//Nach
	bFound = false;
	for( iChain=++lChain.begin();iChain!=lChain.end();++iChain)
	{
		{
			if(iChain->nY==features[referedIdx].y && iChain->nX==features[referedIdx].x)
			{
				list<ChainCode>::iterator it;
				for(it = ++iChain;it!=lChain.end();it++)
				{
					if(it->nValue==5)
					{
						bFound=true;
						ccTemp = *(it);
						features[nCountFeaturePoints].type = FrontNach1;
						features[nCountFeaturePoints].x = ccTemp.nX;
						features[nCountFeaturePoints].y = ccTemp.nY;
						features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
						pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
						referedIdx = nCountFeaturePoints;
						nCountFeaturePoints++;
						//for(int i = ccTemp.nX-2;i>0;--i)
						//{
						//	if(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]>0)
						//	{
						//		if(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i-1]>0)
						//			i--;
						//		features[nCountFeaturePoints].type = FrontNach2;
						//		features[nCountFeaturePoints].x = i;
						//		features[nCountFeaturePoints].y = ccTemp.nY;
						//		//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
						//		pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]=255;
						//		nCountFeaturePoints++;
						//		break;
						//	}
						//}
						break;
					}
				}
				if(bFound)
					break;
			}
		}
	}
	
	//eo
	bFound = false;
	for( iChain=++lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY==features[referedIdx].y && iChain->nX==features[referedIdx].x)
		{
			list<ChainCode>::iterator it;
			for(it = ++iChain;it!=lTurningPoint.end();it++)
			{
				if(it->nValue==7)
				{
					bFound=true;
					ccTemp = *(it);
					features[nCountFeaturePoints].type = FrontEo1;
					features[nCountFeaturePoints].x = ccTemp.nX;
					features[nCountFeaturePoints].y = ccTemp.nY;
					features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
					pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
					referedIdx = nCountFeaturePoints;
					nCountFeaturePoints++;
					for(int i = ccTemp.nX-2;i>0;--i)
					{
						if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i])>0)
						{
							if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i-1])>0)
								i--;
							features[nCountFeaturePoints].type = FrontEo2;
							features[nCountFeaturePoints].x = i;
							features[nCountFeaturePoints].y = ccTemp.nY;
							//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
							pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]=255;
							nCountFeaturePoints++;
							break;
						}
					}
					break;
				}
			}
			if(bFound)
				break;
		}
	}
	//Co^ chan^
	bFound = false;
	int d = 1000;
	for(iChain=lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if( iChain->nY>(Ymax-3*H/4))
		{
			int x = iChain->nX;
			int y = iChain->nY;

			for(int i = x-2; i>0; i--)
			{
				if((unsigned char)(pImageGray->imageData[y*pImageGray->widthStep+i])!=0 )
				{
					if(d>=(x-i+1))
					{
						d = x-i+1;
						ccTemp.nX = x;
						ccTemp.nY = y;
						ccTemp.nValue = iChain->nValue;

					}
					if(d<(x-i+1))
					{
						bFound = true;
						features[nCountFeaturePoints].type = FrontCoChan1;
						features[nCountFeaturePoints].x = ccTemp.nX;
						features[nCountFeaturePoints].y = ccTemp.nY;
						features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
						pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
						referedIdx = nCountFeaturePoints;
						nCountFeaturePoints++;
						for(int i = ccTemp.nX-2;i>0;--i)
						{
							if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i])>0)
							{
								if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i-1])>0)
									i--;
								features[nCountFeaturePoints].type = FrontCoChan2;
								features[nCountFeaturePoints].x = i;
								features[nCountFeaturePoints].y = ccTemp.nY;
								//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
								pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]=255;
								nCountFeaturePoints++;
								break;
							}
						}
					}	
					break;
				}
			}
			if(bFound)
				break;
		}
	}
	//Mat ca' chan^
	bFound = false;
	d = 0;
	for(iChain=lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if( iChain->nY>features[referedIdx].y /*&& iChain->nY>(Ymax-H/2)*/)
		{
			int x = iChain->nX;
			int y = iChain->nY;

			for(int i = x-2; i>0; i--)
			{
				if((unsigned char)(pImageGray->imageData[y*pImageGray->widthStep+i])!=0 )
				{
					if(d<(x-i+1))
					{
						d = x-i+1;
						ccTemp.nX = x;
						ccTemp.nY = y;
						ccTemp.nValue = iChain->nValue;

					}
					if(d>(x-i+1))
					{
						bFound = true;
						features[nCountFeaturePoints].type = FrontMatCaChan1;
						features[nCountFeaturePoints].x = ccTemp.nX;
						features[nCountFeaturePoints].y = ccTemp.nY;
						features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
						pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
						referedIdx = nCountFeaturePoints;
						nCountFeaturePoints++;
						for(int i = ccTemp.nX-2;i>0;--i)
						{
							if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i])>0)
							{
								if((unsigned char)(pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i-1])>0)
									i--;
								features[nCountFeaturePoints].type = FrontMatCaChan2;
								features[nCountFeaturePoints].x = i;
								features[nCountFeaturePoints].y = ccTemp.nY;
								//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
								pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+i]=255;
								nCountFeaturePoints++;
								break;
							}
						}
					}	
					break;
				}
			}
			if(bFound)
				break;
		}
	}
	//Day'
	for(int i=1;i<nCountFeaturePoints;i++)
	{
		if(features[i].type==FrontCoChan2)
		{
			referedIdx=i;
			break;
		}
	}
	bFound = false;
	for(iChain=lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY==features[referedIdx].y && iChain->nX==features[referedIdx].x)
		{
			list<ChainCode>::iterator it;
			for(it=++iChain;it!=lTurningPoint.end();it++)
			{
				if(it->nValue==5)
				{
					bFound=true;
					ccTemp = *(it);
					features[nCountFeaturePoints].type = FrontDay;
					features[nCountFeaturePoints].x = ccTemp.nX;
					features[nCountFeaturePoints].y = ccTemp.nY;
					features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
					pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
					referedIdx = nCountFeaturePoints;
					nCountFeaturePoints++;
					break;
				}
			}
			if(bFound)
				break;
		}
	}

	//nach'2
	for(int i=1;i<nCountFeaturePoints;i++)
		if (features[i].type==FrontEo2)
		{
			referedIdx = i;
			break;
		}

	bFound=false;
	for(iChain=lTurningPoint.begin();iChain!=lTurningPoint.end();++iChain)
	{
		if(iChain->nY==features[referedIdx].y && iChain->nX==features[referedIdx].x)
		{
			list<ChainCode>::iterator it;
			for(it=++iChain;it!=lTurningPoint.end();it++)
			{
				if(it->nValue==5)
				{

					bFound=true;
					ccTemp = *(it);
					features[nCountFeaturePoints].type = FrontNach2;
					features[nCountFeaturePoints].x = ccTemp.nX;
					features[nCountFeaturePoints].y = ccTemp.nY;
					features[nCountFeaturePoints].nCCValue= ccTemp.nValue;
					pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=255;
					referedIdx = nCountFeaturePoints;
					nCountFeaturePoints++;
					break;
				}
			}
			if(bFound)
				break;
		}
	}
	//Rong ban` chan
	int maxX = 0;
	int minX = 10000;
	Feature tempMax;
	Feature tempMin;
	for(int y = Ymax;y>(Ymax - H);--y)
	{
		for(int x = Xmin; x<=Xmax;x++)
		{
			if((unsigned char)(pImageGray->imageData[y*pImageGray->widthStep+x])>0)
			{
				if(maxX<x)
				{
					maxX = x;
					tempMax.x = maxX;
					tempMax.y = y;
				}
				if(minX>x)
				{
					minX = x;
					tempMin.x = minX;
					tempMin.y = y;
				}
			}
		}
	}
	features[nCountFeaturePoints].type = FrontRongChan1;
	features[nCountFeaturePoints].x = tempMax.x;
	features[nCountFeaturePoints].y = tempMax.y;
	//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
	pImageGray->imageData[tempMax.y*pImageGray->widthStep+tempMax.x]=255;
	referedIdx = nCountFeaturePoints;
	nCountFeaturePoints++;
	for(int i = tempMax.x-2;i>0;--i)
	{
		if((unsigned char)(pImageGray->imageData[tempMax.y*pImageGray->widthStep+i])>0)
		{
			if((unsigned char)(pImageGray->imageData[tempMax.y*pImageGray->widthStep+i-1])>0)
				i--;
			features[nCountFeaturePoints].type = FrontRongChan2;
			features[nCountFeaturePoints].x = i;
			features[nCountFeaturePoints].y = tempMax.y;
			//features[nCountFeaturePoints].nCCValue = temp.nCCValue;
			pImageGray->imageData[tempMax.y*pImageGray->widthStep+i]=255;
			nCountFeaturePoints++;
			break;
		}
	}
	
	//got' chan
	
}
int ImageProcessing::GetFeatureHeightFromHead(ImageProcessing ip, FeatureType type)
{
	for (int i=1;i<ip.nCountFeaturePoints;i++)
	{
		if(ip.features[i].type == type)
		{
			return ip.features[i].y-ip.Ymin+1;
		}
	}
}
void ImageProcessing::SetFeatureHeightFromHead( FeatureType typeRight,FeatureType typeLeft, int y,int rightMin, int leftMin)
{
	for(int i=Xmax-rightMin;i>Xmin;i--)
	{
		if((unsigned char)pImageGray->imageData[y*pImageGray->widthStep+i]>0)
		{
			features[nCountFeaturePoints].type = typeRight;
			features[nCountFeaturePoints].x = i;
			features[nCountFeaturePoints].y = y;
			pImageGray->imageData[y*pImageGray->widthStep+i]=255;
			nCountFeaturePoints++;
			break;
		}
	}
	
	for(int i = Xmin+leftMin;i<Xmax;i++)
	{
		
		if((unsigned char)pImageGray->imageData[y*pImageGray->widthStep+i]>0)
		{
			features[nCountFeaturePoints].type = typeLeft;
			features[nCountFeaturePoints].x = i;
			features[nCountFeaturePoints].y = y;
			pImageGray->imageData[y*pImageGray->widthStep+i]=255;
			nCountFeaturePoints++;
			return;
		}
	}
}
void ImageProcessing::GetFeaturesFromSide(ImageProcessing referedIP)
{
	//Xu*o*ng Hong^
	int referedHeight = 0;//height from head in image side
	int convertedHeight = 0;//converted height in image front
	int HeightOfSide = referedIP.Ymax-referedIP.Ymin+1;
	int HeightOfFront = Ymax-Ymin+1;
	int dLeft = 0;
	int dRight = 0;

	referedHeight = GetFeatureHeightFromHead(referedIP,SideXuongHong1);
	convertedHeight = int((float(referedHeight)/float(HeightOfSide))*HeightOfFront)+Ymin;
	dLeft = dRight = (Xmax-Xmin)/4;
	SetFeatureHeightFromHead(FrontXuongHong1,FrontXuongHong2,convertedHeight,dRight,dLeft);

	referedHeight = GetFeatureHeightFromHead(referedIP,SideMong1);
	convertedHeight = int((float(referedHeight)/float(HeightOfSide))*HeightOfFront)+Ymin;
	dLeft = dRight = (Xmax-Xmin)/4;
	SetFeatureHeightFromHead(FrontMong1,FrontMong2,convertedHeight,dRight,dLeft);

	referedHeight = GetFeatureHeightFromHead(referedIP,SideDauGoi1);
	convertedHeight = int((float(referedHeight)/float(HeightOfSide))*HeightOfFront)+Ymin;
	dRight = 1;
	dLeft =(Xmax-Xmin)/2;
	SetFeatureHeightFromHead(FrontDauGoi1,FrontDauGoi2,convertedHeight,dRight,dLeft);

	referedHeight = GetFeatureHeightFromHead(referedIP,SideBapChan1);
	convertedHeight = int((float(referedHeight)/float(HeightOfSide))*HeightOfFront)+Ymin;
	dRight = 1;
	dLeft =(Xmax-Xmin)/2;
	SetFeatureHeightFromHead(FrontBapChan1,FrontBapChan2,convertedHeight,dRight,dLeft);

	referedHeight = GetFeatureHeightFromHead(referedIP,SideTran1);
	convertedHeight = int((float(referedHeight)/float(HeightOfSide))*HeightOfFront)+Ymin;
	dRight = 2;
	dLeft =(Xmax-Xmin)/4;
	SetFeatureHeightFromHead(FrontTran1,FrontTran2,convertedHeight,dRight,dLeft);
	
}
void ImageProcessing::GetFeaturesFromFront(ImageProcessing referedIP)
{
	int referedHeight = 0;//height from head in image side
	int convertedHeight = 0;//converted height in image front
	int HeightOfSide = Ymax-Ymin+1;
	int HeightOfFront = referedIP.Ymax-referedIP.Ymin+1;
	int dLeft = 0;
	int dRight = 0;

	referedHeight = GetFeatureHeightFromHead(referedIP,FrontEo1);
	convertedHeight = int((float(referedHeight)/float(HeightOfFront))*HeightOfSide)+Ymin;
	dLeft = dRight = (Xmax-Xmin)/4;
	SetFeatureHeightFromHead(SideEo1,SideEo2,convertedHeight,dRight,dLeft);
}
bool ImageProcessing::DetectFeaturePoint(int nType)
{
	if(lChain.size()==0)
	{
		cout<<"Do not exist any chain code"<<endl;
		return false;
	}
	nCountFeaturePoints=0;

	//Extract turning points
	
	ChainCode ccRoot;
	ChainCode ccTemp;

	list<ChainCode>::iterator iChain;
	ccRoot = *lChain.begin();
	
	for( iChain=++lChain.begin();iChain!=lChain.end();++iChain)
	{
		ccTemp = (*iChain);
		if (ccTemp.nValue!=ccRoot.nValue && ccTemp.nValue!=(ccRoot.nValue+4)%8)
		{
			ccTemp.nValue = (unsigned char)abs(ccTemp.nValue - ccRoot.nValue);
			lTurningPoint.push_back(ccTemp);
			pImageGray->imageData[ccTemp.nY*pImageGray->widthStep+ccTemp.nX]=120;
		}
	}
	
	if(nType==0)
	{
		ExtractFeaturesforFront();
	}
	else
	{
		ExtractFeaturesforSide();
	}
	
	
}
void ImageProcessing::DrawFeaturePoints()
{//draw feature points
	for(int i=1;i<nCountFeaturePoints;i++)
	{
		DrawMarkerPoint(pImageOri,features[i].x,features[i].y,5,5);
	}
}
void ImageProcessing::PaintBackground()
{
	for(int j=0;j<h;j++)
		for(int i=0;i<w;i++)
		{
			long int idx = j*pImage->widthStep+3*i;
			if((unsigned char)pImage->imageData[idx+0]<90
				&& (unsigned char)pImage->imageData[idx+1]<90
				&& (unsigned char)pImage->imageData[idx+2]<90)
			{
				pImage->imageData[idx+0] = (char)0;
				pImage->imageData[idx+1] = (char)200;
				pImage->imageData[idx+2] = (char)200;
			}
		}
}
void ImageProcessing::Test_PrintChainCode()
{
	
}
HSV* ImageProcessing::GetHSV()
{
	return pHSV;
}

IplImage* ImageProcessing::GetImageObj()
{
	return pImage;
}
IplImage* ImageProcessing::GetImageGrayObj()
{
	return pImageGray;
}
IplImage* ImageProcessing::GetImageOri()
{
	return pImageOri;
}

void ImageProcessing::ExportFeatures( IplImage *img,int type, char* filenamefull, char** fileoutfull )
{
	char *filenameshort;
	filenameshort= new char[90];
	//char *fileoutfull = new char[100];
	char *filetype = new char[4];

	string str = (string)filenamefull;
	string temp;
	int pos;
	pos=str.find_last_of(".");
	temp = (str.substr(pos+1));
	strcpy(filetype,temp.c_str());
	temp = str.substr(0,pos);
	strcpy(filenameshort,temp.c_str());

	//IplImage *img;
	char* text = new char(100);
	text = (type==1)?"SIDE IMAGE":"FRONT IMAGE";
	//cout<<text<<endl;
	//load image
	//ip.LoadImage(filenamefull);
	//ip.PaintBackground();
	img = GetImageObj();
	sprintf(fileoutfull[0],"%s_0_verified.%s",filenameshort,filetype);
	cvSaveImage(fileoutfull[0],img);
	//TestShowImg(img,0);
	//cout<<"Input file: "<<filenamefull<<endl;
	//cout<<"Saved file: "<<fileoutfull<<endl;

	//processing HSV
	RGBtoHSV();
	//loc blue
	FilterOnHSV(150,280,50,255,0,0);
	//loc green
	//ip.FilterOnHSV(150-120,280-120,0,255,0,0);

	HSVtoRGB();
	img = GetImageObj();
	sprintf(fileoutfull[1],"%s_1_Silhouette.%s",filenameshort,filetype);
	cvSaveImage(fileoutfull[1],img);
	//TestShowImg(img,1);
	//cout<<"Saved file : "<<fileoutfull<<endl;

	//extract edge using canny method
	SetupGrayImage();
	MakeCannyEdge();
	img = GetImageGrayObj();
	sprintf(fileoutfull[2],"%s_2_CannyEdge.%s",filenameshort,filetype);
	cvSaveImage(fileoutfull[2],img);
	//TestShowImg(img,2);
	//cout<<"Saved file : "<<fileoutfull<<endl;

	CreateChainCode();
	img = GetImageGrayObj();
	sprintf(fileoutfull[3],"%s_3_ChainBound.%s",filenameshort,filetype);
	cvSaveImage(fileoutfull[3],img);
	//TestShowImg(img,3);
	//cout<<"Saved: "<<fileoutfull<<endl;

	DetectFeaturePoint(type);//detect side
	img = GetImageGrayObj();
	sprintf(fileoutfull[4],"%s_4_Features.%s",filenameshort,filetype);
	cvSaveImage(fileoutfull[4],img);
	//TestShowImg(img,4);
	//cout<<"Saved file : "<<fileoutfull[4]<<endl;

	//img = ip.GetImageOri();
	sprintf(fileoutfull[5],"%s_4_Result.%s",filenameshort,filetype);
	//cout<<fileoutfull[5]<<endl;
	//cvSaveImage(fileoutfull,img);
	//TestShowImg(img,type);
	//cout<<"Xmin = "<<Xmin<<endl;
	//cout<<"Xmax = "<<Xmax<<endl;
	//cout<<"=>Width = "<<Xmax-Xmin<<endl;
	//cout<<"Ymin = "<<Ymin<<endl;
	//cout<<"Ymax = "<<Ymax<<endl;
	//cout<<"=>Height = "<<Ymax-Ymin<<endl;

	//cout<<"# features: "<<nCountFeaturePoints<<endl;
}
char* ImageProcessing::ErrorReport(int ID)
{
	switch(ID)
	{
	case 1: return "Khong mo duoc file";
		break;
	case 51: return "CreateChainCode(): Anh binary chua khoi tao!";
		break;
	case 52: return "CreateChainCode(): Khong tim thay pixel root!";
		break;
	case 53: return "CreateChainCode(): khong the tao chain code vi duong bien bi gian doan";
		break;
	default: "Khong tim thay loi :)";
		break;
	}
}